import numpy as np
import cv2


def compute_gamma_matrix(orig_camera, crop_camera):
    """
    计算将原始图像映射到裁剪图像的变换矩阵Γ
    Γ = Kvirt * R^{-1}_{virt→real} * K^{-1}_orig
    """
    # 1. 计算旋转矩阵 R_{virt→real}
    T_orig = np.linalg.inv(orig_camera.camera_to_world_xf)
    T_crop = np.linalg.inv(crop_camera.camera_to_world_xf)
    T_virt_to_real = T_orig @ np.linalg.inv(T_crop)
    R_virt_to_real = T_virt_to_real[:3, :3]

    # 2. 计算R^{-1}_{virt→real}
    R_inv = np.linalg.inv(R_virt_to_real)

    # 3. 构建内参矩阵
    K_orig = np.array([
        [orig_camera.f[0], 0, orig_camera.c[0]],
        [0, orig_camera.f[1], orig_camera.c[1]],
        [0, 0, 1]
    ])

    K_virt = np.array([
        [crop_camera.f[0], 0, crop_camera.c[0]],
        [0, crop_camera.f[1], crop_camera.c[1]],
        [0, 0, 1]
    ])

    # 4. 计算Γ矩阵
    Gamma = K_virt @ R_inv @ np.linalg.inv(K_orig)
    return Gamma


# 使用OpenCV高效实现
def perspective_crop_image_cv(orig_image, orig_camera, crop_camera):
    Gamma = compute_gamma_matrix(orig_camera, crop_camera)
    return cv2.warpPerspective(
        orig_image,
        Gamma,
        (crop_camera.width, crop_camera.height),
        flags=cv2.INTER_LINEAR,
        # borderMode=cv2.BORDER_REFLECT
    )